#include <api/libchunk/chunk.h>
#include <bus/flash/flash.h>
#include <api/libc/printf.h>
#include <api/libc/string.h>
#include "nn.h"

#ifdef CI_PLUS_SUPPORT
                                                  
#define WRITE_WORD(address,value)  (*(volatile unsigned long *)(address)=value)
#define READ_WORD(address)  (*(volatile unsigned long *)(address))


unsigned char signature[32];
R_RSA_PUBLIC_KEY Public_Key;


#define SHA_HW_256_MODE (1<<30)
#define SHA_HW_FLASH_DMA (1<<1)
#define SHA_HW_FLASH_STA (1)
#define SHA_DIGEST_SWP	(1<<5)
#define SHA_DIGEST_LE	(1<<4)
#define SHA_DMA_OK	(1)
#define SHA_BASE_ADDR 0xb8034000
#define SHA_CTRL	(SHA_BASE_ADDR + 0x300)
#define SHA_STATUS	(SHA_BASE_ADDR + 0x304)
#define SHA_DMA_ADDRESS	(SHA_BASE_ADDR + 0x308)
#define SHA_DMA_LENGTH	(SHA_BASE_ADDR + 0x30c)
#define SHA_DMA_STATUS	(SHA_BASE_ADDR + 0x310)
#define SHA_DIGEST_PORT	(SHA_BASE_ADDR + 0x314)
#define SHA_DIGEST_SEL	(SHA_BASE_ADDR + 0x318)

#define FLASH_DATA_BYTE_SWAP 0x8
 
unsigned int addr_outputlen;
unsigned char addr_output[256];
extern struct sto_device *flash_dev;

int create_sha(unsigned char * src, unsigned long length, unsigned long * addr);

unsigned char rsa_sig[256];

/******************************CRYPT***********************************/

#include <openssl/aes.h>
#include <openssl/evp.h>
#include <api/libci/ci_plus.h>

#define CIPLUS_KEY_PUBLIC_KEY 			15
#define CIPLUS_KEY_MAINCODE_SIGNATURE 	14

#define GET_DWORD(i)           (*(volatile UINT32 *)(OTP_BASE + i))
#define SET_DWORD(i,d)         (*(volatile UINT32 *)(OTP_BASE + i)) = (d)
#define GET_WORD(i)            (*(volatile UINT16 *)(OTP_BASE + i))
#define SET_WORD(i,d)          (*(volatile UINT16 *)(OTP_BASE + i)) = (d)
#define GET_BYTE(i)            (*(volatile UINT8 *)(OTP_BASE + i))
#define SET_BYTE(i,d)          (*(volatile UINT8 *)(OTP_BASE + i)) = (d)

#define OTP_BASE 0xb8000100

#define REG_TIMING  0x0
#define REG_CTRL    0x4
#define REG_ADDR    0x4
#define REG_PDATA   0x6
#define REG_RDATA   0x8
#define REG_SC      0xc
#define REG_CRC_VAL 0xe 
#define REG_ACCESS  (0x5f<<2)
#define OTP_CONFIG  (0x03<<2)

#define OTP_READ_BUSY 1<<27
#define OTP_READ_TRIG 1<<26
#define OTP_PROG_BUSY 1<<25
#define OTP_PROG_TRIG 1<<24
#define OTP_CRC_EN    1<<0
#define OTP_PPROGOTP_PPROG 1<<31

UINT32 ali_otp_read(UINT16 addr)
{
    SET_WORD(REG_ADDR, addr&0xfffc);
    SET_DWORD(REG_CTRL, GET_DWORD(REG_CTRL)|OTP_READ_TRIG);
    while(GET_DWORD(REG_CTRL) & OTP_READ_BUSY);
    return GET_DWORD(REG_RDATA);
}


static RET_CODE decrypt_key(UINT8 *in, UINT8 *out, UINT8 *key, INT32 length)
{
	UINT8 otp_key[16];
	UINT8 decrypted_key[16];
	//UINT8 temp[2][16];
	AES_KEY aes_key;
	UINT32 value;
	UINT32 i;

	//read key from otp
	for(i=0; i<4; i++)
	{
		value = ali_otp_read(0x4d+i*4);
		MEMCPY(otp_key+i*4, &value, 4);
	}

	//decrypt key
	AES_set_decrypt_key(otp_key, 128, &aes_key);
	AES_ecb_encrypt(key, decrypted_key, &aes_key, 0);

	//decrypt input
	AES_set_decrypt_key(decrypted_key, 128, &aes_key);
	while(length>0)
	{
		if(length<16)
		{
			//MEMSET(temp[0], 0, 16);
			//MEMCPY(temp[0], in, length);
			//AES_ecb_encrypt(temp[0], temp[1], &aes_key, 0);
			//MEMCPY(out, temp[1], length);
			MEMCPY(out, in, length);
		}
		else
		{
			AES_ecb_encrypt(in, out, &aes_key, 0);
		}
		in+=16;
		out+=16;
		length -= 16;
	}

	return 0;
}

#define CIPLUSKEY_CHUNK_ID 0x09F60101

typedef struct {
	UINT32 type;
	UINT32 len;
	UINT32 size;
	UINT32 reserved;
} CIPLUS_KEY_HDR;

static RET_CODE find_key(UINT32 type, unsigned long *paddr, CIPLUS_KEY_HDR *phdr)
{
	unsigned long addr, chunk_end;
	UINT32 chunk_len;
	UINT32 ch_id = CIPLUSKEY_CHUNK_ID;

	addr = sto_chunk_goto(&ch_id, 0xFFFFFFFF, 1);
    	if(addr & 0x80000000) 
		return RET_FAILURE;

	chunk_len = sto_fetch_long(addr + CHUNK_LENGTH);

	chunk_end = addr +16 + chunk_len;

	addr += CHUNK_HEADER_SIZE;

	while(addr < chunk_end)
	{

		phdr->type = sto_fetch_long(addr);
		phdr->len = sto_fetch_long(addr+4);
		if(phdr->type == type)
		{
			*paddr = addr+sizeof(CIPLUS_KEY_HDR);
			phdr->size = sto_fetch_long(addr+8);
			//phdr->reserved = sto_fetch_long(addr+12);
			return RET_SUCCESS;
		}
		addr += phdr->len;
	}

	return RET_FAILURE;
}

RET_CODE sys_get_host_key(UINT8 *key, UINT32 *len, UINT32 type)
{
	unsigned long addr;
	CIPLUS_KEY_HDR hdr;
	UINT8 aes_key[16];
	UINT8 *encryped_data;
	struct sto_device *dev = (struct sto_device *)dev_get_by_id(HLD_DEV_TYPE_STO, 0);

	libc_printf("%s - type %d\n",__FUNCTION__,type);

	if(RET_SUCCESS != find_key(CIPLUS_KEY_AES_KEY, &addr, &hdr))
		return RET_FAILURE;

	sto_get_data(dev, aes_key, addr, 16);
	
	if(RET_SUCCESS != find_key(type, &addr, &hdr))
		return RET_FAILURE;

	encryped_data = (UINT8 *)MALLOC(hdr.size);
	//encryped_data = g_ci_tmp_buff;

	sto_get_data(dev, encryped_data, addr, hdr.size);

	decrypt_key(encryped_data, key, aes_key, hdr.size);

	if(len)
		*len = hdr.size;
	
	FREE(encryped_data);

	return RET_SUCCESS;
}
/******************************CRYPT***********************************/

#include <openssl/rsa.h>

unsigned char pubkey[516];

int maincode_verify(unsigned int addr, unsigned int len)
{
	int ret = 0;
	struct sto_device *test_flash_dev;
	unsigned long i = 0;
	RSA *r_vfy;
	int flen;
	
	test_flash_dev = (struct sto_device *)dev_get_by_type(NULL, HLD_DEV_TYPE_STO);
	if (test_flash_dev == NULL)
	{
	    libc_printf("Can't find FLASH device!\n");
	}

	//fetch RSA public key in CI+ chunk
	//sys_get_host_key((UINT8 *)&Public_Key, NULL, CIPLUS_KEY_PUBLIC_KEY);
	sys_get_host_key((UINT8 *)pubkey, NULL, CIPLUS_KEY_PUBLIC_KEY);
	
	//fetch maincode signature in CI+ chunk 
	sys_get_host_key((UINT8 *)&rsa_sig, NULL, CIPLUS_KEY_MAINCODE_SIGNATURE);
	
	//rsapublicfunc(addr_output,&addr_outputlen,rsa_sig,0x100,&Public_Key);
	/********replace rsapublicfunc() with OPENSSL function********/
	r_vfy = RSA_new();
	r_vfy->n = BN_new();
	r_vfy->e = BN_new();
	BN_bin2bn(pubkey+4, 256, r_vfy->n);
	BN_bin2bn(pubkey+4+256+256-3, 3, r_vfy->e);
	flen = RSA_size(r_vfy);
	RSA_public_decrypt(flen, rsa_sig, addr_output, r_vfy, RSA_NO_PADDING);
	/*************************************************************/
	
	create_sha((unsigned char *)addr+SYS_FLASH_BASE_ADDR,len,(unsigned long *)signature);//SHA-256
	
	for(i = 0; i < 32; i += 4)
	{
		if((*(unsigned long *)(addr_output + 0xe0 + i)) != (*(unsigned long *)(signature + i)))
		{
			ret = -1;
			break;
		}
	}
	
	return ret;
}

int create_sha(unsigned char * src, unsigned long length, unsigned long * addr)
{
	int i;
	WRITE_WORD(0xb802e098, ((READ_WORD(0xb802e098)&0xf0ffffff)|0x02000000));
	WRITE_WORD(SHA_STATUS, SHA_DMA_OK);
	WRITE_WORD(SHA_CTRL,SHA_HW_256_MODE|SHA_HW_FLASH_DMA);
	WRITE_WORD(SHA_DMA_ADDRESS,(unsigned long)src);
	WRITE_WORD(SHA_DMA_LENGTH,length);
	WRITE_WORD(SHA_CTRL,SHA_HW_256_MODE|SHA_HW_FLASH_DMA|SHA_HW_FLASH_STA|FLASH_DATA_BYTE_SWAP);
	i=READ_WORD(SHA_DMA_STATUS);
	while(READ_WORD(SHA_STATUS) ==0);
	WRITE_WORD(SHA_STATUS, SHA_DMA_OK);
	for(i=0;i<8;i++)
		{
			WRITE_WORD(SHA_DIGEST_SEL,i|SHA_DIGEST_SWP|SHA_DIGEST_LE);
			*(addr+i)=READ_WORD(SHA_DIGEST_PORT);
		}
	WRITE_WORD(SHA_CTRL,SHA_HW_256_MODE|SHA_HW_FLASH_DMA);
	return 1;
}

#endif

